#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>

#include <lingodroids_bridge/SharedAttention.h>
#include <lingodroids_bridge/Word.h>

#include <fstream>
#include <iostream>
#include <stdlib.h>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
using boost::property_tree::ptree;

class Droid
{
public:
	std::string name;
	double x, y, a;
	std::string word;
	int game_id;

	Droid(std::string name) : name(name)
	{
		nh = new ros::NodeHandle(name);
		unsigned qsize = 1000;
		header.seq = 0;
		header.frame_id = "0";

		ros::Subscriber s;
		sub_pose = nh->subscribe<geometry_msgs::Pose2D>("pose", qsize, &Droid::callback_pose, this);
		sub_word = nh->subscribe<lingodroids_bridge::Word>("word", qsize, &Droid::callback_word, this);
	}

	void callback_pose(const geometry_msgs::Pose2DConstPtr& msg)
	{
		x = msg->x;
		y = msg->y;
		a = msg->theta;
	}

	void callback_word(const lingodroids_bridge::WordConstPtr& msg)
	{
		word = msg->string;
		game_id = msg->game_id;
	}

private:
	ros::NodeHandle *nh;
	std_msgs::Header header;
	ros::Subscriber sub_pose;
	ros::Subscriber sub_word;
};

void init_droids(std::vector<Droid*>& droids, std::string filename);
void process_attention(std::vector<Droid*>& droids, std::vector<lingodroids_bridge::SharedAttention>& attention, float thresh, ros::Publisher& pub);

int main(int argc, char **argv)
{
	if (argc < 2)
	{
		printf("usage: %s config_file\n", argv[0]);
		return -1;
	}

	ptree config, lingo_config, sim_config;
	try
	{
		read_ini(argv[1], config);
		lingo_config = config.get_child("lingodroids");
		sim_config = config.get_child("simulator");
	}
	catch (std::exception const& e)
	{
		printf("Unable to open configuration file - %s\n", e.what());
		return -1;
	}

	ros::init(argc, argv, "lingosim");

	ros::NodeHandle nh;
	unsigned qsize = 1000;

	ros::Publisher pub_attention(nh.advertise<lingodroids_bridge::SharedAttention>(lingo_config.get<std::string>("shared_attention_topic"), qsize));
	std::vector<lingodroids_bridge::SharedAttention> attention;
	float shared_attention_thresh = lingo_config.get<float>("shared_attention_thresh");

	printf("shared_attention_thresh=%f\n", shared_attention_thresh);

	std::vector<Droid*> droids;
	init_droids(droids, sim_config.get<std::string>("rats_file"));

	ros::WallRate r(30.0);

	while(true)
	{
		r.sleep();
		ros::spinOnce();

		process_attention(droids, attention, shared_attention_thresh, pub_attention);
	}

	return 0;
}

void init_droids(std::vector<Droid*>& droids, std::string filename)
{
	std::fstream file(filename.c_str());
	if (!file.is_open())
	{
		printf("could not open %s\n", filename.c_str());
		return;
	}
	std::string line;
	char colour[100];
	while (file.good())
	{
		getline(file, line);
		if (line[0] == '#' || line == "") continue;
		sscanf(line.c_str(), "%s", colour);
		std::string c = "/irat_" + std::string(colour);
		Droid *d = new Droid(c);
		droids.push_back(d);
		printf("monitoring droid '%s'\n", c.c_str());
		if (file.eof()) break;
	}
	file.close();
}

// check for shared attention
void process_attention(std::vector<Droid*>& droids, std::vector<lingodroids_bridge::SharedAttention>& attention, float thresh, ros::Publisher& pub)
{
	static std_msgs::Header header;
	header.stamp = ros::Time::now();
	header.seq++;
	for (unsigned i = 0; i < droids.size(); i++)
	{
		float ix = droids[i]->x;
		float iy = droids[i]->y;
		for (unsigned j = 0; j < droids.size(); j++)
		{
			if (droids[i] == droids[j]) continue;

			std::string iname = droids[i]->name;
			std::string jname = droids[j]->name;
			float dxs = pow(ix - droids[j]->x, 2);
			float dys = pow(iy - droids[j]->y, 2);

			if (sqrt(dxs + dys) < thresh)		// do have shared attention
			{
				bool already_attentive = false;
				for (unsigned k = 0; k < attention.size(); k++)
				{
					if ((iname == attention[k].agent1 && jname == attention[k].agent2)
							|| (iname == attention[k].agent2 && jname == attention[k].agent1))
					{
						already_attentive = true;
						break;
					}
				}
				if (!already_attentive)
				{
					lingodroids_bridge::SharedAttention sa;
					sa.entering = true;
					sa.agent1 = iname;
					sa.agent2 = jname;
					sa.header =  header;
					pub.publish(sa);
					attention.push_back(sa);
					printf("%s and %s entering\n", sa.agent1.c_str(), sa.agent2.c_str());
				}
			}
			else												// don't have shared attention
			{
				for (unsigned k = 0; k < attention.size(); k++)
				{
					if ((iname == attention[k].agent1 && jname == attention[k].agent2)
							|| (iname == attention[k].agent2 && jname == attention[k].agent1))
					{
						lingodroids_bridge::SharedAttention sa = attention[k];
						sa.entering = false;
						sa.header =  header;
						pub.publish(sa);
						printf("%s and %s leaving\n", sa.agent1.c_str(), sa.agent2.c_str());
						attention.erase(attention.begin() + k);
					}
				}
			}
		}
	}
}
